#include "axes.hpp"

Axes::Axes()
{
    x_axis = Vec3d(1.0f, 0.0f, 0.0f);
    y_axis = Vec3d(0.0f, 1.0f, 0.0f);
    z_axis = Vec3d(0.0f, 0.0f, 1.0f);
    _rot_A_A(Vec3d(10.0f, 5.0f, 8.0f), Vec3d(1.0f, 2.0f, 3.0f),15);
}

Axes::Axes(Vec3d rotationAngles, int AXES_ROTATION_ORDER)
{
    Vec3d r = rotationAngles;
    x_axis = Vec3d(1.0f, 0.0f, 0.0f);
    y_axis = Vec3d(0.0f, 1.0f, 0.0f);
    z_axis = Vec3d(0.0f, 0.0f, 1.0f);
    switch(AXES_ROTATION_ORDER)
    {
        case AXES_XYZ:
            x_axis = _rot_G_Z( _rot_G_Y( _rot_G_X( x_axis, r.x ),r.y ),r.z );
            y_axis = _rot_G_Z( _rot_G_Y( _rot_G_X( y_axis, r.x ),r.y ),r.z );
            z_axis = _rot_G_Z( _rot_G_Y( _rot_G_X( z_axis, r.x ),r.y ),r.z );
        break;
        case AXES_XZY:
            x_axis = _rot_G_Y( _rot_G_Z( _rot_G_X( x_axis, r.x ),r.z ),r.y );
            y_axis = _rot_G_Y( _rot_G_Z( _rot_G_X( y_axis, r.x ),r.z ),r.y );
            z_axis = _rot_G_Y( _rot_G_Z( _rot_G_X( z_axis, r.x ),r.z ),r.y );
        break;
        case AXES_YXZ:
            x_axis = _rot_G_Z( _rot_G_X( _rot_G_Y( x_axis, r.y ),r.x ),r.z );
            y_axis = _rot_G_Z( _rot_G_X( _rot_G_Y( y_axis, r.y ),r.x ),r.z );
            z_axis = _rot_G_Z( _rot_G_X( _rot_G_Y( z_axis, r.y ),r.x ),r.z );
        break;
        case AXES_YZX:
            x_axis = _rot_G_X( _rot_G_Z( _rot_G_Y( x_axis, r.y ),r.z ),r.x );
            y_axis = _rot_G_X( _rot_G_Z( _rot_G_Y( y_axis, r.y ),r.z ),r.x );
            z_axis = _rot_G_X( _rot_G_Z( _rot_G_Y( z_axis, r.y ),r.z ),r.x );
        break;
        case AXES_ZXY:
            x_axis = _rot_G_Y( _rot_G_X( _rot_G_Z( x_axis, r.z ),r.x ),r.y );
            y_axis = _rot_G_Y( _rot_G_X( _rot_G_Z( y_axis, r.z ),r.x ),r.y );
            z_axis = _rot_G_Y( _rot_G_X( _rot_G_Z( z_axis, r.z ),r.x ),r.y );
        break;
        case AXES_ZYX:
            x_axis = _rot_G_X( _rot_G_Y( _rot_G_Z( x_axis, r.z ),r.y ),r.x );
            y_axis = _rot_G_X( _rot_G_Y( _rot_G_Z( y_axis, r.z ),r.y ),r.x );
            z_axis = _rot_G_X( _rot_G_Y( _rot_G_Z( z_axis, r.z ),r.y ),r.x );
        break;

        // now the relative cases
        case AXES_XYZr:
            x_axis = _rot_L_Z( _rot_L_Y( _rot_L_X( x_axis, r.x ),r.y ),r.z );
            y_axis = _rot_L_Z( _rot_L_Y( _rot_L_X( y_axis, r.x ),r.y ),r.z );
            z_axis = _rot_L_Z( _rot_L_Y( _rot_L_X( z_axis, r.x ),r.y ),r.z );
        break;
        case AXES_XZYr:
            x_axis = _rot_L_Y( _rot_L_Z( _rot_L_X( x_axis, r.x ),r.z ),r.y );
            y_axis = _rot_L_Y( _rot_L_Z( _rot_L_X( y_axis, r.x ),r.z ),r.y );
            z_axis = _rot_L_Y( _rot_L_Z( _rot_L_X( z_axis, r.x ),r.z ),r.y );
        break;
        case AXES_YXZr:
            x_axis = _rot_L_Z( _rot_L_X( _rot_L_Y( x_axis, r.y ),r.x ),r.z );
            y_axis = _rot_L_Z( _rot_L_X( _rot_L_Y( y_axis, r.y ),r.x ),r.z );
            z_axis = _rot_L_Z( _rot_L_X( _rot_L_Y( z_axis, r.y ),r.x ),r.z );
        break;
        case AXES_YZXr:
            x_axis = _rot_L_X( _rot_L_Z( _rot_L_Y( x_axis, r.y ),r.z ),r.x );
            y_axis = _rot_L_X( _rot_L_Z( _rot_L_Y( y_axis, r.y ),r.z ),r.x );
            z_axis = _rot_L_X( _rot_L_Z( _rot_L_Y( z_axis, r.y ),r.z ),r.x );
        break;
        case AXES_ZXYr:
            x_axis = _rot_L_Y( _rot_L_X( _rot_L_Z( x_axis, r.z ),r.x ),r.y );
            y_axis = _rot_L_Y( _rot_L_X( _rot_L_Z( y_axis, r.z ),r.x ),r.y );
            z_axis = _rot_L_Y( _rot_L_X( _rot_L_Z( z_axis, r.z ),r.x ),r.y );
        break;
        case AXES_ZYXr:
            x_axis = _rot_L_X( _rot_L_Y( _rot_L_Z( x_axis, r.z ),r.y ),r.x );
            y_axis = _rot_L_X( _rot_L_Y( _rot_L_Z( y_axis, r.z ),r.y ),r.x );
            z_axis = _rot_L_X( _rot_L_Y( _rot_L_Z( z_axis, r.z ),r.y ),r.x );
        break;
    }
    x_axis.normalize();
    y_axis.normalize();
    z_axis.normalize();
    checkOrthogonality();

}


Axes::Axes(Vec3d xAxis, Vec3d yAxis, Vec3d zAxis)
{
    x_axis = xAxis.normalized();
    y_axis = yAxis.normalized();
    z_axis = zAxis.normalized();
    checkOrthogonality();
}

//support functions
Vec3d Axes::_rot_G_X(Vec3d input, double angle)
{
    Vec3d result(0.0f, 0.0f, 0.0f);
    result.x = input.x*1.0f + input.y*0.0f              + input.z*0.0f;
    result.y = input.x*0.0f + input.y*cos(rad(angle)) - input.z*sin(rad(angle));
    result.z = input.x*0.0f + input.y*sin(rad(angle)) + input.z*cos(rad(angle));
    return result;
     
}
Vec3d Axes::_rot_G_Y(Vec3d input, double angle)
{
    Vec3d result(0.0f, 0.0f, 0.0f);
    result.x = input.x*cos(rad(angle)) + input.y*0.0f - input.z*sin(rad(angle));
    result.y = input.x*0.0f               + input.y*1.0f + input.z*0.0f;
    result.z = input.x*sin(rad(angle)) + input.y*0.0f + input.z*cos(rad(angle));

    return result;
}

Vec3d Axes::_rot_G_Z(Vec3d input, double angle)
{
    Vec3d result(0.0f, 0.0f, 0.0f);
    result.x =  input.x*cos(rad(angle)) + input.y*sin(rad(angle)) + input.z*0.0f;
    result.y = -input.x*sin(rad(angle)) + input.y*cos(rad(angle)) + input.z*0.0f;
    result.z =  input.x*0.0f            + input.y*0.0f              + input.z*1.0f;
    return result;
}

Vec3d Axes::_rot_L_X(Vec3d input, double angle)
{
    return _rot_A_A(input, x_axis, angle);
}

Vec3d Axes::_rot_L_Y(Vec3d input, double angle)
{
    return _rot_A_A(input, y_axis, angle);
}

Vec3d Axes::_rot_L_Z(Vec3d input, double angle)
{
    return _rot_A_A(input, z_axis, angle);
}



Vec3d Axes::_rot_A_A(Vec3d input, Vec3d axis, double angle)
{
    if (fabs(angle) <= EPSILON)
        return input;

    Vec3d result = input;
    // determine angle between axis and the XY-plane
    double xy_angle;
    xy_angle = atan2(axis.z, axis.x)*180.0f/PI;
    // with it, we rotate the input vector
    result = _rot_G_Y(result, -xy_angle);
    axis = _rot_G_Y(axis, -xy_angle);

    // Our axis is now aligned with the XY plane
    // Now we find the angle between the axis and the Y-axis
    double y_angle;
    y_angle = atan2(axis.x, axis.y)*180.0f/PI;
    // now we rotate around the Z-axis
    result = _rot_G_Z(result, -y_angle);
//    axis = _rot_G_Z(axis, -y_angle);

    // our axis is now aligned with the Y-axis
    // so we perform the rotation in a CW direction
    result = _rot_G_Y(result, angle);

    // and then we undo the rotations IN REVERSE!
    result = _rot_G_Z(result, y_angle);
    result = _rot_G_Y(result, xy_angle);

    return result;
}

void Axes::checkOrthogonality()
{
    try
    {
        double xy = abs(x_axis.dot(y_axis));
        double xz = abs(x_axis.dot(z_axis));
        double yz = abs(y_axis.dot(z_axis));
        if (xy > EPSILON)
            throw "EXCEPTION: AXES NOT ORTHOGONAL";
        if (xz > EPSILON)
            throw "EXCEPTION: AXES NOT ORTHOGONAL";
        if (yz > EPSILON)
            throw "EXCEPTION: AXES NOT ORTHOGONAL";
    }
    catch(std::string ex)
    {
        std::cout << ex;
    }
}


// accessors
Vec3d Axes::getAxis(int AXES__AXIS)
{
    switch(AXES__AXIS)
    {
    case AXES_X_AXIS:
        return x_axis;
    break;
    case AXES_Y_AXIS:
        return y_axis;
    break;
    case AXES_Z_AXIS:
        return z_axis;
    break;
    default:
        return Vec3d(0.0f, 0.0f, 0.0f);
    break;
    }
}

Vec3d Axes::xAxis()
{
    return x_axis;
}

Vec3d Axes::yAxis()
{
    return y_axis;
}

Vec3d Axes::zAxis()
{
    return z_axis;
}

// modifiers
void Axes::rotateAxesAroundGlobalAxis(int AXES__AXIS, double angle)
{
    if (fabs(angle) <= EPSILON)
        return;
    switch(AXES__AXIS)
    {
    case AXES_X_AXIS:
        x_axis = _rot_G_X(x_axis, angle);
        y_axis = _rot_G_X(y_axis, angle);
        z_axis = _rot_G_X(z_axis, angle);
    break;
    case AXES_Y_AXIS:
        x_axis = _rot_G_Y(x_axis, angle);
        y_axis = _rot_G_Y(y_axis, angle);
        z_axis = _rot_G_Y(z_axis, angle);
    break;
    case AXES_Z_AXIS:
        x_axis = _rot_G_Z(x_axis, angle);
        y_axis = _rot_G_Z(y_axis, angle);
        z_axis = _rot_G_Z(z_axis, angle);
    break;
    }
    x_axis.normalize();
    y_axis.normalize();
    z_axis.normalize();
    checkOrthogonality();
}

void Axes::rotateAxesAroundLocalAxis(int AXES__AXIS, double angle)
{
    if (fabs(angle) <= EPSILON)
        return;
    switch(AXES__AXIS)
    {
    case AXES_X_AXIS:
        y_axis = _rot_L_X(y_axis, angle);
        z_axis = _rot_L_X(z_axis, angle);
    break;
    case AXES_Y_AXIS:
        x_axis = _rot_L_Y(x_axis, angle);
        z_axis = _rot_L_Y(z_axis, angle);
    break;
    case AXES_Z_AXIS:
        x_axis = _rot_L_Z(x_axis, angle);
        y_axis = _rot_L_Z(y_axis, angle);
    break;
    }
    x_axis.normalize();
    y_axis.normalize();
    z_axis.normalize();
    checkOrthogonality();
}

void Axes::rotateAxesAroundArbitraryAxis(Vec3d axis, double angle)
{
    if (fabs(angle) <= EPSILON)
        return;
    x_axis = _rot_A_A(x_axis, axis, angle);
    y_axis = _rot_A_A(y_axis, axis, angle);
    z_axis = _rot_A_A(z_axis, axis, angle);

    x_axis.normalize();
    y_axis.normalize();
    z_axis.normalize();
    checkOrthogonality();
}


// helpers
Vec3d Axes::rotatePointAroundLocalAxis(Vec3d point, int AXES__AXIS, double angle)
{
    if (fabs(angle) <= EPSILON)
        return point;
    switch(AXES__AXIS)
    {
    case AXES_X_AXIS:
        return _rot_L_X(point, angle);
    break;
    case AXES_Y_AXIS:
        return _rot_L_Y(point, angle);
    break;
    case AXES_Z_AXIS:
        return _rot_L_Z(point, angle);
    break;
    }
    return point;
}

Vec3d Axes::rotatePointAroundGlobalAxis(Vec3d point, int AXES__AXIS, double angle)
{
    if (fabs(angle) <= EPSILON)
        return point;
    switch(AXES__AXIS)
    {
    case AXES_X_AXIS:
        return _rot_G_X(point, angle);
    break;
    case AXES_Y_AXIS:
        return _rot_G_Y(point, angle);
    break;
    case AXES_Z_AXIS:
        return _rot_G_Z(point, angle);
    break;
    }
    return point;
}

Vec3d Axes::getGlobalPointFromLocalPoint(Vec3d point)
{
    Vec3d res(0.0f, 0.0f, 0.0f);
    
    res = x_axis*point.x + y_axis*point.y + z_axis*point.z;

    return res;
}

Vec3d Axes::getLocalPointFromGlobalPoint(Vec3d point)
{
    Vec3d res(0.0f, 0.0f, 0.0f);
    
    res.x = point.dot(x_axis);
    res.y = point.dot(y_axis);
    res.z = point.dot(z_axis);

    return res;
}
